// Include files.
#include "TestDriveSubsystem.hpp"

TestDriveSubsystem::TestDriveSubsystem() :
	MecanumDriveSubsystem
	(
		new CANJaguar(3),
		new CANJaguar(2),
		new CANJaguar(5),
		new CANJaguar(6),
		true
	)
{
	SetControlMode(ControlModeSpeed);
	SetEncodersCodesPerRevolution(360, 250, 250, 250);
	SetPIDs(ControlModeSpeed, 0.1, 0.005, 0);
	SetPIDs(ControlModeDistance, 20.0, 0.002, 0);
	SetMaximumSpeed(250);
}
